/********************************************************************
	created:	2008/10/01
	created:	1:10:2008   19:33
	filename: 	h:\SimoneEngine\qianye\src\qylib\math\qyQuat.h
	author:		Xiong Jiayu
	copyright:  
	purpose:	
*********************************************************************/
#pragma once

#ifndef __QYQUAT_H__
#define __QYQUAT_H__

#include "qyMath.h"
#include "qyMatrix3.h"
struct Matrix4;

/*
===========================================
struct Quat
===========================================
*/
struct Quat
{
public:
	float			x;
	float			y;
	float			z;
	float			w;
public:
					Quat( void );
					Quat( const float x, const float y, const float z, const float w );
					Quat( const Quat &q );
	void			Set( const float x, const float y, const float z, const float w );

	float			operator[]( const int index ) const;
	float &			operator[]( const int index );
	Quat			operator-( void ) const;
	Quat &			operator=( const Quat &a );
	Quat			operator+( const Quat &a ) const;
	Quat &			operator+=( const Quat &a );
	Quat			operator-( const Quat &a ) const;
	Quat &			operator-=( const Quat &a );
	Quat			operator*( const Quat &a ) const;
	Quat &			operator*=( const Quat &a );
	Quat			operator*( const float f ) const;
	Quat &			operator*=( const float f );
	Vector3			operator*( const Vector3 &v ) const;

	friend	Quat	operator*( const float f, const Quat &q );
	friend	Vector3	operator*( const Vector3 &v, const Quat &q );

	bool			Compare( const Quat &a ) const;
	bool			Compare( const Quat &a , const float epsilon ) const;
	bool			operator==( const Quat &a ) const;
	bool			operator!=( const Quat &a ) const;

	Quat			Inverse( void ) const;
	Quat &			InverseSelf( void );
	void			Identity( void );
	float			Length( void ) const;
	Quat			Normalize( void ) const;
	Quat &			NormalizeSelf( void );

	float			CalcW( void ) const;
	float			CalcSelfW( void );
	Quat & 			Slerp( const Quat &from, const Quat &to, float t );
	Quat &			SlerpFast( const Quat &from, const Quat &to, float t );
	void			Rotate( Vector3 &v ) const;
    void            RotateFrom( Vector3 &dest, const Vector3 &v ) const;

	void			SetRotationAxis( const Vector3 &axis, float angle );
	Matrix3			ToMatrix3( void ) const;
    void            ToMatrix3( Matrix3 &m ) const;
	void			ToMatrix4( Matrix4 &m ) const;
	void			FromMatrix3( const Matrix3 &m );
	void			FromMatrix4( const Matrix4 &m );
};

//-----------------------------------------------------------------------
QY_INLINE Quat::Quat()
{

}

//-----------------------------------------------------------------------
QY_INLINE Quat::Quat(const float _x, const float _y, const float _z, const float _w)
{
	this->x = _x;
	this->y = _y;
	this->z = _z;
	this->w = _w;
}

//-----------------------------------------------------------------------
QY_INLINE Quat::Quat( const Quat &q )
{
	x = q.x; y = q.y; z = q.z; w = q.w;
}

//-----------------------------------------------------------------------
QY_INLINE float Quat::operator []( const int index ) const
{
	return ( &x )[ index ];
}

//-----------------------------------------------------------------------
QY_INLINE float & Quat::operator []( const int index )
{
	return ( &x )[ index ];
}

//-----------------------------------------------------------------------
QY_INLINE Quat Quat::operator -( void ) const
{
	return Quat( -x, -y, -z, w );
}

//-----------------------------------------------------------------------
QY_INLINE Quat & Quat::operator =( const Quat &a )
{
	this->x = a.x;
	this->y = a.y;
	this->z = a.z;
	this->w = a.w;
	return ( *this );
}

//-----------------------------------------------------------------------
QY_INLINE Quat Quat::operator +( const Quat &a ) const
{
	return Quat( x + a.x, y + a.y, z + a.z, w + a.w );
}

//-----------------------------------------------------------------------
QY_INLINE Quat & Quat::operator +=( const Quat &a )
{
	x += a.x;
	y += a.y;
	z += a.z;
	w += a.w;
	return ( *this );
}

//-----------------------------------------------------------------------
QY_INLINE Quat Quat::operator -( const Quat &a ) const
{
	return Quat( x - a.x, y - a.y, z - a.z, w - a.w );
}

//-----------------------------------------------------------------------
QY_INLINE Quat & Quat::operator -=( const Quat &a ) 
{
	x -= a.x;
	y -= a.y;
	z -= a.z;
	w -= a.w;
	return (*this);
}

//-----------------------------------------------------------------------
QY_INLINE Quat Quat::operator *(const Quat &b ) const
{
	/*
	return Quat( w*a.x + x*a.w + y*a.z - z*a.y,
		w*a.y + y*a.w + z*a.x - x*a.z,
		w*a.z + z*a.w + w*a.y - y*a.x,
		w*a.w - x*a.x - y*a.y - z*a.z );
		*/
	return Quat( 
		b.w * this->x + b.x * this->w + b.y * this->z - b.z * this->y,
		b.w * this->y + b.y * this->w + b.z * this->x - b.x * this->z,
		b.w * this->z + b.z * this->w + b.x * this->y - b.y * this->x,
		b.w * this->w - b.x * this->x - b.y * this->y - b.z * this->z 
		);
}

//-----------------------------------------------------------------------
QY_INLINE Vector3 Quat::operator *( const Vector3&v ) const
{
	/*
	float xx = x * x;
	float zz = z * z;
	float ww = w * w;
	float yy = y * y;

	float xw2 = x*w*2.0f;
	float xy2 = x*y*2.0f;
	float xz2 = x*z*2.0f;
	float yw2 = y*w*2.0f;
	float yz2 = y*z*2.0f;
	float zw2 = z*w*2.0f;

	return Vector3(
		( xx - yy - zz + ww )	* a.x	+ ( xy2 + zw2 )			* a.y	+ ( xz2 - yw2 )			* a.z,
		( xy2 - zw2 )			* a.x	+ ( -xx + yy - zz + ww )* a.y	+ ( yz2 + xw2 )			* a.z,
		( xz2 + yw2 )			* a.x	+ ( yz2 - xw2 )			* a.y	+ ( -xx - yy + zz + ww )* a.z );

	*/
	Vector3 out,r2;
	r2.x=(y*v.z-z*v.y)+w*v.x;
	r2.y=(z*v.x-x*v.z)+w*v.y;
	r2.z=(x*v.y-y*v.x)+w*v.z;
	out.x=(r2.z*y-r2.y*z); out.x+=out.x+v.x;
	out.y=(r2.x*z-r2.z*x); out.y+=out.y+v.y;
	out.z=(r2.y*x-r2.x*y); out.z+=out.z+v.z;
	return out;
	
}

//-----------------------------------------------------------------------
QY_INLINE Quat Quat::operator *( const float f ) const
{
	return Quat( x * f, y * f, z * f, w * f );
}

//-----------------------------------------------------------------------
QY_INLINE Quat & Quat::operator *=( const float f )
{
	x *= f;
	y *= f;
	z *= f;
	w *= f;
	return ( *this );
}

//-----------------------------------------------------------------------
QY_INLINE Quat & Quat::operator *=( const Quat &a )
{
	*this = *this * a;
	return ( *this );
}

//-----------------------------------------------------------------------
QY_INLINE Quat operator*( const float f, const Quat &q )
{
	return q * f;
}

//-----------------------------------------------------------------------
QY_INLINE Vector3 operator*( const Vector3 &v, const Quat &q )
{
	return q * v;
}

//-----------------------------------------------------------------------
QY_INLINE bool Quat::Compare(const Quat &a) const
{
	return ( ( x == a.x ) && ( y == a.y ) && ( z == a.z ) && ( w == a.w ) );
}

//-----------------------------------------------------------------------
QY_INLINE bool Quat::Compare(const Quat &a , const float epsilon) const
{
	if ( fabsf( x - a.x ) > epsilon )
		return false;
	if ( fabsf( y - a.y ) > epsilon )
		return false;
	if ( fabsf( z - a.z ) > epsilon )
		return false;
	if ( fabsf( w - a.w ) > epsilon )
		return false;
	return true;
}

//-----------------------------------------------------------------------
QY_INLINE bool Quat::operator ==( const Quat &a ) const
{
	return Compare( a );
}

//-----------------------------------------------------------------------
QY_INLINE bool Quat::operator !=( const Quat &a ) const
{
	return !Compare( a );
}

//-----------------------------------------------------------------------
QY_INLINE void Quat::Set(const float x, const float y, const float z, const float w)
{
	this->x = x;
	this->y = y;
	this->z = z;
	this->w = w;
}

//-----------------------------------------------------------------------
QY_INLINE Quat Quat::Inverse( void ) const
{
	return Quat( -x, -y, -z, w );
}

//-----------------------------------------------------------------------
QY_INLINE Quat & Quat::InverseSelf( void )
{
	x = -x; y = -y; z = -z;
	return (*this);
}

//-----------------------------------------------------------------------
QY_INLINE float Quat::Length() const
{
	float len = x*x + y*y + z*z + w*w;
	return sqrtf( len );
}

//-----------------------------------------------------------------------
QY_INLINE Quat Quat::Normalize( void ) const
{
	float len;
	len = this->Length();
	assert( len != 0.0f );
	float invLen = 1.0f / len;
	return Quat( x * invLen, y * invLen, z * invLen, w * invLen );
}

//-----------------------------------------------------------------------
QY_INLINE Quat & Quat::NormalizeSelf( void )
{
	float len;
	len = this->Length();
	assert( len != 0.0f );
	float invLen = 1.0f / len;
	x *= invLen;
	y *= invLen;
	z *= invLen;
	w *= invLen;
	return ( *this );
}

//-----------------------------------------------------------------------
QY_INLINE float Quat::CalcW() const
{
	return sqrtf( fabsf( 1.0f - ( x*x + y*y + z*z ) ) );
}

//-----------------------------------------------------------------------
QY_INLINE void Quat::Identity()
{
	x = 0;
	y = 0;
	z = 0;
	w = 1;
}

//-----------------------------------------------------------------------
QY_INLINE Matrix3 Quat::ToMatrix3() const
{
	/*
	Matrix3	m;
	Vector3 v2( x * 2, y * 2, z * 2 );
	double xx=1-v2.x*x;		double yy=v2.y*y;		double xw=v2.x*w;
	double xy=v2.y*x;		double yz=v2.z*y;		double yw=v2.y*w;
	double xz=v2.z*x;		double zz=v2.z*z;		double zw=v2.z*w;
	m[0][0]=float(1-yy-zz);				m[1][0]=float(xy-zw);			m[2][0]=float(xz+yw);
	m[0][1]=float(xy+zw);				m[1][1]=float(xx-zz);			m[2][1]=float(yz-xw);
	m[0][2]=float(xz-yw);				m[1][2]=float(yz+xw);			m[2][2]=float(xx-yy);
	return m;
	*/
	Matrix3 mat;
	double	wx, wy, wz;
	double	xx, yy, yz;
	double	xy, xz, zz;
	double	x2, y2, z2;

	x2 = x + x;
	y2 = y + y;
	z2 = z + z;

	xx = x * x2;
	xy = x * y2;
	xz = x * z2;

	yy = y * y2;
	yz = y * z2;
	zz = z * z2;

	wx = w * x2;
	wy = w * y2;
	wz = w * z2;

	mat[ 0 ][ 0 ] = 1.0f - float( yy + zz );
	mat[ 1 ][ 0 ] = float( xy - wz );
	mat[ 2 ][ 0 ] = float( xz + wy );

	mat[ 0 ][ 1 ] = float( xy + wz );
	mat[ 1 ][ 1 ] = 1.0f - float( xx + zz );
	mat[ 2 ][ 1 ] = float( yz - wx );

	mat[ 0 ][ 2 ] = float( xz - wy );
	mat[ 1 ][ 2 ] = float( yz + wx );
	mat[ 2 ][ 2 ] = 1.0f - float( xx + yy );

	return mat;
}

//-----------------------------------------------------------------------
QY_INLINE void Quat::ToMatrix3( Matrix3 &mat ) const
{
	/*Vector3 v2( x * 2, y * 2, z * 2 );
	double xx=1-v2.x*x;		double yy=v2.y*y;		double xw=v2.x*w;
	double xy=v2.y*x;		double yz=v2.z*y;		double yw=v2.y*w;
	double xz=v2.z*x;		double zz=v2.z*z;		double zw=v2.z*w;
	m[0][0]=float(1-yy-zz);				m[1][0]=float(xy-zw);			m[2][0]=float(xz+yw);
	m[0][1]=float(xy+zw);				m[1][1]=float(xx-zz);			m[2][1]=float(yz-xw);
	m[0][2]=float(xz-yw);				m[1][2]=float(yz+xw);			m[2][2]=float(xx-yy);
	*/
	
	double	wx, wy, wz;
	double	xx, yy, yz;
	double	xy, xz, zz;
	double	x2, y2, z2;

	x2 = x + x;
	y2 = y + y;
	z2 = z + z;

	xx = x * x2;
	xy = x * y2;
	xz = x * z2;

	yy = y * y2;
	yz = y * z2;
	zz = z * z2;

	wx = w * x2;
	wy = w * y2;
	wz = w * z2;

	mat[ 0 ][ 0 ] = 1.0f - float( yy + zz );
	mat[ 1 ][ 0 ] = float( xy - wz );
	mat[ 2 ][ 0 ] = float( xz + wy );

	mat[ 0 ][ 1 ] = float( xy + wz );
	mat[ 1 ][ 1 ] = 1.0f - float( xx + zz );
	mat[ 2 ][ 1 ] = float( yz - wx );

	mat[ 0 ][ 2 ] = float( xz - wy );
	mat[ 1 ][ 2 ] = float( yz + wx );
	mat[ 2 ][ 2 ] = 1.0f - float( xx + yy );
	/*
	float	wx, wy, wz;
	float	xx, yy, yz;
	float	xy, xz, zz;
	float	x2, y2, z2;

	x2 = x + x;
	y2 = y + y;
	z2 = z + z;

	xx = x * x2;
	xy = x * y2;
	xz = x * z2;

	yy = y * y2;
	yz = y * z2;
	zz = z * z2;

	wx = w * x2;
	wy = w * y2;
	wz = w * z2;

	mat[ 0 ][ 0 ] = 1.0f - ( yy + zz );
	mat[ 1 ][ 1 ] = xy - wz;
	mat[ 2 ][ 2 ] = xz + wy;

	mat[ 0 ][ 1 ] = xy + wz;
	mat[ 1 ][ 1 ] = 1.0f - ( xx + zz );
	mat[ 2 ][ 1 ] = yz - wx;

	mat[ 0 ][ 2 ] = xz - wy;
	mat[ 1 ][ 2 ] = yz + wx;
	mat[ 2 ][ 2 ] = 1.0f - ( xx + yy );
	*/
}

//-----------------------------------------------------------------------
QY_INLINE void Quat::SetRotationAxis( const Vector3 &axis, float angle )
{
	float   thetaOver2 = angle * .5f;
	float   sinThetaOver2 = sinf(thetaOver2);

	// Set the values

	w = cosf( thetaOver2 );
	x = axis.x * sinThetaOver2;
	y = axis.y * sinThetaOver2;
	z = axis.z * sinThetaOver2;
}

//-----------------------------------------------------------------------
QY_INLINE void Quat::Rotate( Vector3 &v ) const
{
	Vector3 a = v;
	float xxzz = x*x - z*z;
	float wwyy = w*w - y*y;

	float xw2 = x*w*2.0f;
	float xy2 = x*y*2.0f;
	float xz2 = x*z*2.0f;
	float yw2 = y*w*2.0f;
	float yz2 = y*z*2.0f;
	float zw2 = z*w*2.0f;

	return v.Set(
		(xxzz + wwyy)*a.x		+ (xy2 + zw2)*a.y		+ (xz2 - yw2)*a.z,
		-(xy2 - zw2)*a.x			- (xxzz - wwyy)*a.y		- (yz2 + xw2)*a.z,
		(xz2 + yw2)*a.x			+ (yz2 - xw2)*a.y		+ (wwyy - xxzz)*a.z
		);
}

//-----------------------------------------------------------------------
QY_INLINE void Quat::RotateFrom( Vector3 &dest, const Vector3 &v ) const
{
    const Vector3 & a = v;
    float xxzz = x*x - z*z;
    float wwyy = w*w - y*y;

    float xw2 = x*w*2.0f;
    float xy2 = x*y*2.0f;
    float xz2 = x*z*2.0f;
    float yw2 = y*w*2.0f;
    float yz2 = y*z*2.0f;
    float zw2 = z*w*2.0f;

    return dest.Set(
        (xxzz + wwyy)*a.x		+ (xy2 + zw2)*a.y		+ (xz2 - yw2)*a.z,
        -(xy2 - zw2)*a.x			- (xxzz - wwyy)*a.y		- (yz2 + xw2)*a.z,
        (xz2 + yw2)*a.x			+ (yz2 - xw2)*a.y		+ (wwyy - xxzz)*a.z
        );
}

//-----------------------------------------------------------------------
QY_INLINE void Quat::FromMatrix3( const Matrix3 &mat )
{
	float		trace;
	float		s;
	float		t;
	int     	i;
	int			j;
	int			k;

	static int 	next[ 3 ] = { 1, 2, 0 };

	trace = mat[ 0 ][ 0 ] + mat[ 1 ][ 1 ] + mat[ 2 ][ 2 ];

	if ( trace > 0.0f ) {

		t = trace + 1.0f;
		s = ( 1.0f / sqrtf( t ) ) * 0.5f;

		w = s * t;
		x = ( mat[ 1 ][ 2 ] - mat[ 2 ][ 1 ] ) * s;
		y = ( mat[ 2 ][ 0 ] - mat[ 0 ][ 2 ] ) * s;
		z = ( mat[ 0 ][ 1 ] - mat[ 1 ][ 0 ] ) * s;

	} else {

		i = 0;
		if ( mat[ 1 ][ 1 ] > mat[ 0 ][ 0 ] ) {
			i = 1;
		}
		if ( mat[ 2 ][ 2 ] > mat[ i ][ i ] ) {
			i = 2;
		}
		j = next[ i ];
		k = next[ j ];

		t = ( mat[ i ][ i ] - ( mat[ j ][ j ] + mat[ k ][ k ] ) ) + 1.0f;
		s = 1/ sqrtf( t ) * 0.5f;

		(*this)[i] = s * t;
		(*this)[3] = ( mat[ j ][ k ] - mat[ k ][ j ] ) * s;
		(*this)[j] = ( mat[ i ][ j ] + mat[ j ][ i ] ) * s;
		(*this)[k] = ( mat[ i ][ k ] + mat[ k ][ i ] ) * s;
	}
}

//-----------------------------------------------------------------------
QY_INLINE Quat &Quat::Slerp( const Quat &from, const Quat &to, float t ) 
{
	float cosom, absCosom, sinom, omega, scale0, scale1;

	if ( t <= 0.0f ) {
		*this = from;
		return *this;
	}

	if ( t >= 1.0f ) {
		*this = to;
		return *this;
	}

	if ( from == to ) {
		*this = to;
		return *this;
	}

	cosom = from.x * to.x + from.y * to.y + from.z * to.z + from.w * to.w;
	absCosom = fabs( cosom );

	if ( ( 1.0f - absCosom ) > 1e-6f )
	{
		omega = acos( absCosom );
		sinom = 1.0f / sin( omega );
		scale0 = sin( ( 1.0f - t ) * omega ) * sinom;
		scale1 = sin( t * omega ) * sinom;

	} 
	else {
		scale0 = 1.0f - t;
		scale1 = t;
	}

	scale1 = ( cosom >= 0.0f ) ? scale1 : -scale1;

	x = scale0 * from.x + scale1 * to.x;
	y = scale0 * from.y + scale1 * to.y;
	z = scale0 * from.z + scale1 * to.z;
	w = scale0 * from.w + scale1 * to.w;

	return *this;
}

//-----------------------------------------------------------------------
QY_INLINE Quat &Quat::SlerpFast( const Quat &from, const Quat &to, float t ) 
{
	float cosom, scale0, scale1, s;

	if ( t <= 0.0f ) {
		*this = from;
		return *this;
	}

	if ( t >= 1.0f ) {
		*this = to;
		return *this;
	}

	if ( from == to ) {
		*this = to;
		return *this;
	}

	cosom = from.x * to.x + from.y * to.y + from.z * to.z + from.w * to.w;

	scale0 = 1.0f - t;
	scale1 = ( cosom >= 0.0f ) ? t : -t;

	x = scale0 * from.x + scale1 * to.x;
	y = scale0 * from.y + scale1 * to.y;
	z = scale0 * from.z + scale1 * to.z;
	w = scale0 * from.w + scale1 * to.w;

	s = 1 / sqrtf( x * x + y * y + z * z + w * w );

	x *= s;
	y *= s;
	z *= s;
	w *= s;

	return *this;
}

#endif
